﻿#include "RobotArm/robotarm.h"

#include <algorithm>


#pragma execution_character_set("utf-8")

RobotArm::RobotArm(QObject *parent) :
    QObject(parent)
{
    //qDebug() << "RobotArm() thread " << this->thread();
    //qDebug() << "parent thread " << parent->thread();
    //timer_get_robot_state_ = new QTimer(this);
    //connect(timer_get_robot_state_,&QTimer::timeout,this,&RobotArm::getRobotState);
    //timer_get_robot_state_->start(300);
    control_socket_ = new QTcpSocket(this);
}

/* wrong
float hex2Float(QByteArray hexValue){

    QString hexString(hexValue.toHex());
    int hexInt = hexString.toInt(nullptr,16);
    float result = *(float*)&hexInt;
    return result;
}
*/

/* wrong
double hex64_10(QByteArray byteDate) {
    char* pData = byteDate.data();
    //符号位  1位
    int sign = int(pData[0] >> 7);
    //指数位  11位
    int e = int(((pData[0] & 0x7F) << 4) & 0x0FF0 | (pData[1] & 0xF0) >> 4) - 1023;
    //小数位  52位
    double  m = pow(2, -52) *
        (pow(256, 6) * double(pData[1] & 0x0F) +
         pow(256, 5) * double(pData[2]) +
         pow(256, 4) * double(pData[3]) +
         pow(256, 3) * double(pData[4]) +
         pow(256, 2) * double(pData[5]) +
         256 * double(pData[6]) +
         double(pData[7])) + 1;
    double res = pow(-1, sign) * m * pow(2, e);
    qDebug() << byteDate.toHex() << QString::number(res, 'f', 24);
    return res;

}
*/

//
bool RobotArm::connectToHost(std::string host, int port){

    //qDebug() << "connectTo Robot arm Host! thread is " << this->thread();

    if(control_socket_->isOpen())
    {
        return true;
    }

    //socket.connectToHost(host, port); //连接机械臂
    control_socket_->connectToHost(QString::fromStdString(host), port);
    if(!control_socket_->waitForConnected(3000))
    {
        return false;
    }
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "Connected to 192.168.5.244:8055>"
    //         << "Ready to read and write!";

    //if(connectToMonitorHost(host,8056)){
    //    qDebug() << "Connect Monitor port success!";
        emit sig_start_monitor();

    //}

    return true;
}

bool RobotArm::connectToMonitorHost(std::string host, int port){

   //bool state = robot_arm_monitor_->connectToMonitorHost(host,port);

   return true;
}

//
bool RobotArm::disconnectToHost(){

    control_socket_->close();
    return true;
}
//

void RobotArm::robot_sleep(unsigned int msec)
{
QTime dieTime = QTime::currentTime().addMSecs(msec);
while( QTime::currentTime() < dieTime )
QCoreApplication::processEvents(QEventLoop::AllEvents, 100);
}

bool RobotArm::robotArmInit(){

    if(!control_socket_->isValid())
    {
        //QMessageBox::critical(this, "错误\t", "未连接到机械臂!\t");
        return false;
    }

    //清除报警
    bool state = false;
    state = clearAlarm();
    if(state == false) return false;

    //同步伺服电机状态
    state = syncMotorStatus();
    if(state == false) return false;

    //打开伺服使能
    state = set_servo_status(true);
    if(state == false) return false;

    //编码器零位校准
    state = calibrate_encoder_zero_position();
    if(state == false) return false;

    return state;
}
//

bool RobotArm::clearAlarm(){

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.insert("method",QJsonValue("clearAlarm")); //清除报警

    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        robot_sleep(7000);
        return state;
    }else{
        return false;
    }

    //QTest::qSleep(7000); //延时3秒有问题，延时7秒没问题
}

bool RobotArm::syncMotorStatus(){

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.insert("method",QJsonValue("syncMotorStatus")); //同步伺服电机状态

    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        robot_sleep(1000);
        return state;
    }else{
        return false;
    }

    //QTest::qSleep(1000);

}

bool RobotArm::set_servo_status(bool state){

    bool rn;
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    if(state){
        QJsonObject params = {{"status",1}};
        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("set_servo_status")); //打开伺服使能
        robot_sleep(1000);
        rn = execSpecifiedCommand(JsonCommand);
    }else{
        QJsonObject params = {{"status",0}};
        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("set_servo_status")); //打开伺服使能
        robot_sleep(1000);
        rn = execSpecifiedCommand(JsonCommand);
    }

    if(rn == true){
        robot_sleep(1000);
        return state;
    }else{
        return false;
    }

}

bool RobotArm::calibrate_encoder_zero_position(){

    // bug , check encoder state first;
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.remove("params");
    JsonCommand.insert("method",QJsonValue("calibrate_encoder_zero_position")); //编码器零位校准
    robot_sleep(1000);
    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        robot_sleep(1000);
        return state;
    }else{
        return false;
    }

    //QTest::qSleep(1000);
}

bool RobotArm::execSpecifiedCommand(QJsonObject &_JsonCommand)
{
    //qDebug() << "execSpecifed thread " << this->thread();

    //send command
    robotCommand.setObject(_JsonCommand);
    QByteArray command = robotCommand.toJson(QJsonDocument::Compact);
    qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');

    //read command
    control_socket_->waitForReadyRead(); //注释掉这句话会出现Error: ASSERT failure in QList<T>::operator[]: "index out of range"
    qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
             << "RECV ASCII FROM 192.168.5.244:8055>";
    QByteArray robotResponse = control_socket_->readAll(); // << '\n';
    qDebug() << "robotResponse " << robotResponse;

    // parse command
    bool result = false;
    QJsonParseError jsonError;
    QJsonDocument parseResponse = QJsonDocument::fromJson(robotResponse, &jsonError);
    if (!parseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        QJsonObject object = parseResponse.object();
        if (object.contains("result"))
        {
            QJsonValue value = object["result"];

            QString value_string = value.toString();

            //qDebug() << "value " << value_string;

            if(value_string == "true"){
                return true;
            }else{
                return false;
            }
        }
        else{
            return false;
        }
    }
    else{
        return false;
    }

}

//执行movebypath; return 路点个数
bool RobotArm::execMoveByPathCommand(QJsonObject& _JsonCommand){
    //qDebug() << "execSpecifed thread " << this->thread();

    //send command
    robotCommand.setObject(_JsonCommand);
    QByteArray command = robotCommand.toJson(QJsonDocument::Compact);
    qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');

    //read command
    control_socket_->waitForReadyRead(); //注释掉这句话会出现Error: ASSERT failure in QList<T>::operator[]: "index out of range"
    qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
             << "RECV ASCII FROM 192.168.5.244:8055>";
    QByteArray robotResponse = control_socket_->readAll(); // << '\n';
    qDebug() << "robotResponse " << robotResponse;

    // parse command
    bool result = false;
    QJsonParseError jsonError;
    QJsonDocument parseResponse = QJsonDocument::fromJson(robotResponse, &jsonError);
    if (!parseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        QJsonObject object = parseResponse.object();
        if (object.contains("result"))
        {
            QJsonValue value = object["result"];

            int value_int = value.toInt();

            //qDebug() << "value " << value_string;

            if(value_int != -1){
                return true;
            }else{
                return false;
            }
        }
        else{
            return false;
        }
    }
    else{
        return false;
    }
}

QString RobotArm::getRobotState()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.remove("params");
    JsonCommand.insert("method",QJsonValue("getRobotState"));
    robotCommand.setObject(JsonCommand);
    QByteArray command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    QByteArray robotStateResponse = control_socket_->readAll();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "RECV ASCII TO 192.168.5.244:8055>" << robotStateResponse << '\n';
    //robotStateResponse.chop(2);
    return robotStateParse(robotStateResponse);
}

QString RobotArm::robotStateParse(QByteArray &_robotStateResponse)
{
    QJsonParseError jsonError;
    QJsonDocument parseRobotState = QJsonDocument::fromJson(_robotStateResponse, &jsonError);
    if (!parseRobotState.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        if (parseRobotState.isObject())
        {
            QJsonObject object = parseRobotState.object();
            if (object.contains("result"))
            {
                //QJsonValue value = object["result"];
                QRegExp rx("(\\d)");
                //QString str = value.toString();
                rx.indexIn((object["result"].toString()), 0);
                robotState.clear();
                robotState = rx.cap(1);
            }
        }
    }

    return robotState;
}

bool RobotArm::move_to_pose(MoveType type,QString pose_name)
{
//    if(robot_common_poses_.find(pose_name)
//            == robot_common_poses_.end()) return false;

//    RobotPoseJoint pose = robot_common_poses_.at(pose_name);

//    bool state = false;

//    if(type == MoveType::LINE){
//        state = move_by_line(pose);
//    }
//    else if(type == MoveType::JOINT){
//        state = move_by_joint(pose);
//    }

    return true;
}

bool RobotArm::move_to_pose(RobotPoseJoint pose,
                            MoveType move_type,
                            double speed,
                            int acceleration,
                            int deceleration,
                            BlockMotion blockmotion){

    //移动方式
    bool state = false;
    if(move_type == MoveType::LINE){
        state = move_by_line(pose, speed, acceleration, deceleration, blockmotion);
    }
    else if(move_type == MoveType::JOINT){
        state = move_by_joint(pose, speed, acceleration, deceleration, blockmotion);
    }

    //阻塞方式
    robot_sleep(200);
    if(blockmotion == BlockMotion::LOCKED){

        while(1){
            robot_sleep(100);
            //qDebug() << "robot_arm_monitor_->robot_state "
            //         << robot_arm_monitor_->robot_state;
            if(robot_arm_monitor_->robot_state == 0){
                break;
            }

            //急停检测
            if(robot_arm_monitor_->robot_state == 2){
                QMessageBox::warning(nullptr, "提示\t", "急停按钮按下\t");
                return false;
            }

            //远程手自动切换
            if(robot_arm_monitor_->robot_mode != 2){
                QMessageBox::warning(nullptr, "提示\t", "切换至手动模式，停止自动运行\t");
                return false;
            }

            //AGV急停按下
            if(agv_emergency_stop == true){
                qDebug() << "AGV急停按钮按下，停止自动运行";
                //QMessageBox::warning(nullptr, "提示\t", "AGV急停按下，停止自动运行\t");
                return false;
            }
        }
    }

    return state;
}

bool RobotArm::move_to_pose(RobotPoseXYZ pose,
                            MoveType move_type,
                            double speed,
                            int acceleration,
                            int deceleration,
                            BlockMotion blockmotion){

    //空间直角坐标转换到关节坐标
    RobotPoseJoint joint_pose;
    if(!getIK(pose,joint_pose)){
        return false;
    }

    //移动方式
    bool state = false;
    if(move_type == MoveType::LINE){
        state = move_by_line(joint_pose, speed, acceleration, deceleration, blockmotion);
    }
    else if(move_type == MoveType::JOINT){
        state = move_by_joint(joint_pose, speed, acceleration, deceleration, blockmotion);
    }

    //阻塞方式
    robot_sleep(200);
    if(blockmotion == BlockMotion::LOCKED){

        while(1){
            robot_sleep(100);
            if(robot_arm_monitor_->robot_state == 0){
                break;
            }

            //急停检测
            if(robot_arm_monitor_->robot_state == 2){
                return false;
            }

            //远程手自动切换
            if(robot_arm_monitor_->robot_mode != 2){
                QMessageBox::warning(nullptr, "提示\t", "切换至手动模式，停止自动运行\t");
                return false;
            }

            //AGV急停按下
            if(agv_emergency_stop == true){
                 qDebug() << "AGV急停按钮按下，停止自动运行";
                //QMessageBox::warning(nullptr, "提示\t", "AGV急停按下，停止自动运行\t");
                return false;
            }
        }
    }

    return state;
}

bool RobotArm::move_to_common_pose(QString pose_name,
                                   MoveType type,
                                   double speed,
                                   int acceleration,
                                   int deceleration,
                                   BlockMotion blockmotion){
    
    //qDebug() << "pose_name " << pose_name;
    if(robot_common_poses_.find(pose_name) == robot_common_poses_.end()) return false;
    
    RobotPoseJoint pose_data = robot_common_poses_.at(pose_name);

    bool state = move_to_pose(pose_data, type, speed, acceleration, deceleration, blockmotion);
    if(state == false) return false;

    //qDebug() << "Move Done";
    return true;
}

bool RobotArm::move_to_station_pose(QString station_id,
                                    QString pose_name,
                                    MoveType type,
                                    double speed,
                                    int acceleration,
                                    int deceleration,
                                    BlockMotion blockmotion){

    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()) return false;

    auto data_poses = robot_station_poses_.at(station_id);

    RobotPoseXYZ pose_data;
    if(data_poses.find(pose_name) == data_poses.end()){
        return false;
    }
    else{
        pose_data = data_poses.at(pose_name);

        //qDebug() << "pose_data " << pose_data.x;
    }


    bool state = move_to_pose(pose_data,
                              type,
                              speed,
                              acceleration,
                              deceleration,
                              blockmotion);
    if(state == false) return false;

    /*
    if(blockmotion == BlockMotion::LOCKED){

        while(1){
            robot_sleep(10);
            if(robot_arm_monitor_->robot_state == 0){
                break;
            }
        }
    }
    */

    return state;
}

bool RobotArm::move_by_joint(RobotPoseJoint& pose,
                             double speed,
                             int acceleration,
                             int deceleration,
                             BlockMotion blockmotion){
    
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    QJsonArray targetPositionByJoint = {pose.joint1,
                                        pose.joint2,
                                        pose.joint3,
                                        pose.joint4,
                                        pose.joint5,
                                        pose.joint6};
    QJsonObject params = {{"speed",speed},
                          {"acc", acceleration},
                          {"dec", deceleration},
                          {"cond_type", 0},
                          {"cond_num", 5},
                          {"cond_value", 1},
                          {"targetPos",targetPositionByJoint}};

    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveByJoint"));

    return execSpecifiedCommand(JsonCommand);

}

bool RobotArm::move_by_line(RobotPoseJoint& pose,
                            double speed,
                            int acceleration,
                            int deceleration,
                            BlockMotion blockmotion){

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    QJsonArray targetPositionByJoint = {pose.joint1,
                                        pose.joint2,
                                        pose.joint3,
                                        pose.joint4,
                                        pose.joint5,
                                        pose.joint6};
    QJsonObject params = {{"speed",speed},
                          {"acc", acceleration},
                          {"dec", deceleration},
                          {"cond_type", 0},
                          {"cond_num", 5},
                          {"cond_value", 1},
                          {"targetPos",targetPositionByJoint}};

    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveByLine"));

    return execSpecifiedCommand(JsonCommand);
}


RobotPoseJoint RobotArm::getRobotPoseJoint(){

    RobotPoseJoint pose = {robot_arm_monitor_->robot_joint_pose[0],
                          robot_arm_monitor_->robot_joint_pose[1],
                          robot_arm_monitor_->robot_joint_pose[2],
                          robot_arm_monitor_->robot_joint_pose[3],
                          robot_arm_monitor_->robot_joint_pose[4],
                          robot_arm_monitor_->robot_joint_pose[5]};

    return pose;
}

RobotPoseXYZ RobotArm::getRobotPoseXYZ(){
    RobotPoseXYZ pose = {robot_arm_monitor_->robot_xyz_pose[0],
                          robot_arm_monitor_->robot_xyz_pose[1],
                          robot_arm_monitor_->robot_xyz_pose[2],
                          robot_arm_monitor_->robot_xyz_pose[3],
                          robot_arm_monitor_->robot_xyz_pose[4],
                          robot_arm_monitor_->robot_xyz_pose[5]};

    return pose;
}

bool RobotArm::stop_movement(){
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.insert("method",QJsonValue("stop")); //清除报警

    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        return true;
    }else{
        return false;
    }

}

//暂停
bool RobotArm::pause_movement(){

    //只有当前状态为运动时，才发送暂停信号
    if(robot_arm_monitor_->robot_state != 3){
        return false;
    }

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.insert("method",QJsonValue("pause")); //清除报警

    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        return true;
    }else{
        return false;
    }
}
//恢复运动
bool RobotArm::run_movement(){


    //只有当前状态为暂停时，才发送恢复信号
    if(robot_arm_monitor_->robot_state != 1){
        return false;
    }

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
    JsonCommand.insert("method",QJsonValue("run")); //清除报警

    bool state = execSpecifiedCommand(JsonCommand);

    if(state == true){
        return true;
    }else{
        return false;
    }
}

void RobotArm::moveToXMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {-1.0*GeneralSpeed,0.0,0.0,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

bool RobotArm::isConnected(){

    if(control_socket_->isOpen())
    {
        return true;
    }else{
        return false;
    }
}

void RobotArm::moveToXPlus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {GeneralSpeed,0.0,0.0,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToYMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,-1.0*GeneralSpeed,0.0,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToYPlus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,GeneralSpeed,0.0,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToZMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,-1.0*GeneralSpeed,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToZPlus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,1.0*GeneralSpeed,0.0,0.0,0.0};
    params = {{"v",velocity},{"acc",GeneralACC},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToRollMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,-30.0,0.0,0.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToRollPlus()
{

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,30.0,0.0,0.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToPitchMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,-30.0,0.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToPitchPlus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,30.0,0.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToYawMinus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,0.0,-30.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToYawPlus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,0.0,30.0};
    params = {{"v",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedl"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint1Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {-1.0,0.0,0.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint1Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {1.0,0.0,0.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint2Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,-1.0,0.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint2Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,1.0,0.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint3Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,-1.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint3Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,1.0,0.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint4Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,-1.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //        << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint4Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,1.0,0.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint5Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,-1.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint5Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,1.0,0.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint6Minus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,0.0,-1.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}

void RobotArm::moveToJoint6Plus()
{
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

    velocity = {0.0,0.0,0.0,0.0,0.0,1.0};
    params = {{"vj",velocity},{"acc",15},{"t",60}};
    JsonCommand.insert("params",QJsonValue(params));
    JsonCommand.insert("method",QJsonValue("moveBySpeedj"));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << control_socket_->readAll() << '\n';
}


bool RobotArm::isContainCommonPose(QString name){

    if(robot_common_poses_.find(name) != robot_common_poses_.end()){
        return false;
    }
    else{
        return true;
    }
}

bool RobotArm::isContainStationPose(QString station_id, QString name){

    if(robot_station_poses_.find(station_id) != robot_station_poses_.end()){
        return false;
    }
    else{

        auto it = robot_station_poses_.at(station_id);

        if(it.find(name) == it.end()){
            return false;
        }
        else{
            return true;
        }
    }
}

std::map<QString, RobotPoseJoint> RobotArm::getRobotCommonPose(){

    return robot_common_poses_;
}

std::map<QString,std::map<QString, RobotPoseXYZ>> RobotArm::getRobotStationPose(){

    return robot_station_poses_;
}

bool RobotArm::add_common_pose(QString name, RobotPoseJoint pose){

    if(robot_common_poses_.find(name) == robot_common_poses_.end()){
        robot_common_poses_.emplace(name, pose);
    }
    else{
        //update
        robot_common_poses_.at(name) = pose;
    }

    return true;
}


bool RobotArm::add_station_pose(QString station_id, QString name, RobotPoseXYZ pose){

    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){

        std::map<QString,RobotPoseXYZ> data;
        data.emplace(name,pose);
        //insert new
        robot_station_poses_.emplace(station_id, data);
    }
    else{
        auto& data_poses = robot_station_poses_.at(station_id);

        if(data_poses.find(name) == data_poses.end()){
            //insert new
            data_poses.emplace(name, pose);
        }else{
            //update
            data_poses.at(name) = pose;
        }
    }

    return true;

}

bool RobotArm::remove_common_pose(QString name){

    if(robot_common_poses_.find(name) == robot_common_poses_.end())
    {
        qDebug() << "this point do not exist in robot_common_pose";
        return true;
    }
    else{
        robot_common_poses_.erase(name);
    }

}

bool RobotArm::remove_station_pose(QString station_id, QString name){

    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){
        qDebug() << "this station do not exist ";
        return true;
    }
    else{
        auto data_poses = robot_station_poses_.at(station_id);

        if(data_poses.find(name) == data_poses.end()){
            qDebug() << "this pose do not exist in station " << station_id;
            return true;
        }
        else{
            data_poses.erase(name);
            return true;
        }
    }

}

//逆解，XYZ坐标转JOINT坐标
bool RobotArm::getIK(RobotPoseXYZ& pose_xyz, RobotPoseJoint& pose_joint){

    QJsonObject request_IK = { {"jsonrpc",QJsonValue("2.0")},
                               {"method",QJsonValue("inverseKinematic")},
                               {"id",QJsonValue(1)}};

    RobotPoseJoint reference_pose = getRobotPoseJoint();

    QJsonArray referencePos = {reference_pose.joint1,
                               reference_pose.joint2,
                               reference_pose.joint3,
                               reference_pose.joint4,
                               reference_pose.joint5,
                               reference_pose.joint6};

    //qDebug() << "referencePos " << referencePos;

    QJsonArray targetPose = {pose_xyz.x,
                             pose_xyz.y,
                             pose_xyz.z,
                             pose_xyz.rx,
                             pose_xyz.ry,
                             pose_xyz.rz};

    //qDebug() << "targetPose " << targetPose;

    QJsonObject tempparams = {{"targetPose",
                               QJsonValue(targetPose)},
                              {"referencePos",QJsonValue(referencePos)},
                              {"unit_type",QJsonValue(0)}};

    request_IK.insert("params",QJsonValue(tempparams));
    robotCommand.setObject(request_IK);
    command = robotCommand.toJson(QJsonDocument::Compact);

    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    QByteArray robotJointPoseResponse = control_socket_->readAll();

    //QTest::qSleep(1000);
    QJsonArray calculatePos;
    QJsonParseError jsonError;
    QJsonDocument parserobotJointPoseResponse = QJsonDocument::fromJson(robotJointPoseResponse, &jsonError);

    if (!parserobotJointPoseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        if (parserobotJointPoseResponse.isObject())
        {
            QJsonObject object = parserobotJointPoseResponse.object();
            if (object.contains("result"))
            {
                QJsonValue value = object["result"];
                QRegExp rx("(-?\\d+\\.?((\\d+)?))");
                QString str = value.toString();
                int pos = 0;
                while ((pos = rx.indexIn(str, pos)) != -1)
                {
                    calculatePos.append(rx.cap(1).toDouble());
                    pos += rx.matchedLength();
                }
            }
        }
    }

    //qDebug() << "calculatePos " << calculatePos;

    if(calculatePos.size() == 6){
        pose_joint.joint1 = calculatePos.at(0).toDouble();
        pose_joint.joint2 = calculatePos.at(1).toDouble();
        pose_joint.joint3 = calculatePos.at(2).toDouble();
        pose_joint.joint4 = calculatePos.at(3).toDouble();
        pose_joint.joint5 = calculatePos.at(4).toDouble();
        pose_joint.joint6 = calculatePos.at(5).toDouble();
        return true;
    }
    else{
        return false;
    }

}

//正解， JOINT坐标 转 XYZ坐标
bool RobotArm::getFK(RobotPoseJoint pose_joint, RobotPoseXYZ& pose_xyz){

    QJsonObject request_FK = { {"jsonrpc",QJsonValue("2.0")},
                               {"method",QJsonValue("positiveKinematic")},
                               {"id",QJsonValue(1)}};

    QJsonArray targetPose = {pose_joint.joint1,
                             pose_joint.joint2,
                             pose_joint.joint3,
                             pose_joint.joint4,
                             pose_joint.joint5,
                             pose_joint.joint6};

    //qDebug() << "targetPose " << targetPose;

    QJsonObject tempparams = {{"targetPos",
                               QJsonValue(targetPose)},
                              {"unit_type",QJsonValue(0)}};

    request_FK.insert("params",QJsonValue(tempparams));
    robotCommand.setObject(request_FK);
    command = robotCommand.toJson(QJsonDocument::Compact);

    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    QByteArray robotJointPoseResponse = control_socket_->readAll();

    //QTest::qSleep(1000);
    QJsonArray calculatePos;
    QJsonParseError jsonError;
    QJsonDocument parserobotJointPoseResponse = QJsonDocument::fromJson(robotJointPoseResponse, &jsonError);

    if (!parserobotJointPoseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        if (parserobotJointPoseResponse.isObject())
        {
            QJsonObject object = parserobotJointPoseResponse.object();
            if (object.contains("result"))
            {
                QJsonValue value = object["result"];
                QRegExp rx("(-?\\d+\\.?((\\d+)?))");
                QString str = value.toString();
                int pos = 0;
                while ((pos = rx.indexIn(str, pos)) != -1)
                {
                    calculatePos.append(rx.cap(1).toDouble());
                    pos += rx.matchedLength();
                }
            }
        }
    }

    //qDebug() << "calculatePos " << calculatePos;

    if(calculatePos.size() == 6){
        pose_xyz.x = calculatePos.at(0).toDouble();
        pose_xyz.y = calculatePos.at(1).toDouble();
        pose_xyz.z = calculatePos.at(2).toDouble();
        pose_xyz.rx = calculatePos.at(3).toDouble();
        pose_xyz.ry = calculatePos.at(4).toDouble();
        pose_xyz.rz = calculatePos.at(5).toDouble();
        return true;
    }
    else{
        return false;
    }
    return true;
}

void RobotArm::set_robot_monitor(RobotArmMonitor *monitor, QString host){
    robot_arm_monitor_ = monitor;
    robot_arm_monitor_->host_ip = host;
}

bool RobotArm::set_DO(int id, int value){

    //send command
    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"method",QJsonValue("setOutput")},{"id",QJsonValue(1)}};
    params = {};
    params = {{"addr",QJsonValue(id)},{"status",QJsonValue(value)}};
    JsonCommand.insert("params",QJsonValue(params));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    QByteArray setResult = control_socket_->readAll();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << setResult;

    //check command send is success
    QJsonParseError jsonError;
    QJsonDocument parseResponse = QJsonDocument::fromJson(setResult, &jsonError);
    if (!parseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        QJsonObject object = parseResponse.object();
        if (object.contains("result"))
        {
            QJsonValue value = object["result"];

            QString value_string = value.toString();

            //qDebug() << "value " << value_string;

            if(value_string == "true"){
                return true;
            }else{
                return false;
            }
        }
        else{
            return false;
        }
    }
    else{
        return false;
    }
    return true;
}

bool RobotArm::get_DI(int id, int& data){

    QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"method",QJsonValue("getInput")},{"id",QJsonValue(1)}};
    params = {{"addr",id}};
    JsonCommand.insert("params",QJsonValue(params));
    robotCommand.setObject(JsonCommand);
    command = robotCommand.toJson(QJsonDocument::Compact);
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz") << "Send ASCII TO 192.168.5.244:8055>" << command;
    control_socket_->write(command+'\n');
    control_socket_->waitForReadyRead();
    QByteArray DIResult = control_socket_->readAll();
    //qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss zzz")
    //         << "RECV ASCII FROM 192.168.5.244:8055>" << DIResult << '\n';

    // parse command
    QJsonParseError jsonError;
    QJsonDocument parseResponse = QJsonDocument::fromJson(DIResult, &jsonError);
    if (!parseResponse.isNull() && (jsonError.error == QJsonParseError::NoError))
    {
        QJsonObject object = parseResponse.object();
        if (object.contains("result"))
        {
            QJsonValue value = object["result"];

            QString value_string = value.toString();

            //qDebug() << "value " << value_string;

            data = value_string.toInt();
            return true;
        }
        else{
            return false;
        }
    }
    else{
        return false;
    }
}

bool RobotArm::set_acceleration_value(int value){
    return true;
}

bool RobotArm::set_deceleration_value(int value){
    return true;
}

bool RobotArm::copy_station_data(QString copy_station_id, QString target_station_id)
{
    if(robot_station_poses_.find(copy_station_id) == robot_station_poses_.end()){
        return false;
    }

    auto data = robot_station_poses_.at(copy_station_id);

    if(robot_station_poses_.find(target_station_id) == robot_station_poses_.end()){
        robot_station_poses_.emplace(target_station_id, data);
    }
    else{
        robot_station_poses_.at(target_station_id) = data;
    }
}

bool RobotArm::getRobotPoseXYZ(QString station_id, QString pose_name , RobotPoseXYZ& pose){

    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){
        return false;
    }

    auto poseData = robot_station_poses_.at(station_id);

    if(poseData.find(pose_name) == poseData.end()){
        return false;
    }

    pose = poseData.at(pose_name);

    return true;
}

void RobotArm::set_arm_speed(int value){
     GeneralSpeed = value;
}
void RobotArm::set_arm_acc(int value){
     GeneralACC = value;
}
void RobotArm::set_arm_dec(int value){
     GeneralDEC = value;
}

bool RobotArm::update_common_pose(QString name, RobotPoseJoint pose){

    if(robot_common_poses_.find(name) == robot_common_poses_.end()){
        robot_common_poses_.emplace(name,pose);
    }else{
        robot_common_poses_.at(name) = pose;
    }

    return true;
}

bool RobotArm::update_station_pose(QString station_id, QString name, RobotPoseXYZ pose){

    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){

        std::map<QString,RobotPoseXYZ> data;
        data.emplace(name, pose);
        //insert new
        robot_station_poses_.emplace(station_id, data);
    }
    else{
        auto& data_poses = robot_station_poses_.at(station_id);

        if(data_poses.find(name) == data_poses.end()){
            //insert new
            data_poses.emplace(name, pose);
        }else{
            //update
            data_poses.at(name) = pose;
        }
    }

    return true;
}

bool RobotArm::getRobotStationPose(QString station_id, QString name, RobotPoseXYZ& pose){
    if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){
        return false;
    }

    auto station_pose_data = robot_station_poses_.at(station_id);

    if(station_pose_data.find(name) == station_pose_data.end()){
        return false;
    }
    else{

        pose = station_pose_data.at(name);
        return true;
    }
}


bool RobotArm::getRobotCommonPose(QString name, RobotPoseJoint& pose){

    if(robot_common_poses_.find(name) == robot_common_poses_.end()){
        return false;
    }

    pose = robot_common_poses_.at(name);
    return true;
}


bool RobotArm::is_current_pose(QString pose_name, float distance){

    if(robot_common_poses_.find(pose_name) == robot_common_poses_.end()){
        return false;
    }

    RobotPoseXYZ current_xyz_pose = getRobotPoseXYZ();

    current_xyz_pose.rx = current_xyz_pose.rx * M_PI / 180.0;
    current_xyz_pose.ry = current_xyz_pose.ry * M_PI / 180.0;
    current_xyz_pose.rz = current_xyz_pose.rz * M_PI / 180.0;

//    qDebug() << "current_xyz_pose x " << current_xyz_pose.x;
//    qDebug() << "current_xyz_pose y " << current_xyz_pose.y;
//    qDebug() << "current_xyz_pose z " << current_xyz_pose.z;
//    qDebug() << "current_xyz_pose rx " << current_xyz_pose.rx;
//    qDebug() << "current_xyz_pose ry " << current_xyz_pose.ry;
//    qDebug() << "current_xyz_pose rz " << current_xyz_pose.rz;

    RobotPoseJoint target_joint_pose = robot_common_poses_.at(pose_name);



    RobotPoseXYZ target_xyz_pose;
    //正解
    if(getFK(target_joint_pose, target_xyz_pose) == false){
        return false;
    }

    target_xyz_pose.rx = target_xyz_pose.rx * M_PI / 180.0;
    target_xyz_pose.ry = target_xyz_pose.ry * M_PI / 180.0;
    target_xyz_pose.rz = target_xyz_pose.rz * M_PI / 180.0;


//    qDebug() << "target_xyz_pose x " << target_xyz_pose.x;
//    qDebug() << "target_xyz_pose y " << target_xyz_pose.y;
//    qDebug() << "target_xyz_pose z " << target_xyz_pose.z;
//    qDebug() << "target_xyz_pose rx " << target_xyz_pose.rx;
//    qDebug() << "target_xyz_pose ry " << target_xyz_pose.ry;
//    qDebug() << "target_xyz_pose rz " << target_xyz_pose.rz;


    //距离检测

    tf::Vector3 reference_pose_t(current_xyz_pose.x,current_xyz_pose.y,current_xyz_pose.z);
    tf::Quaternion reference_pose_q;
    reference_pose_q.setRPY(current_xyz_pose.rx,current_xyz_pose.ry,current_xyz_pose.rz);
    tf::Transform reference_pose_tf(reference_pose_q, reference_pose_t);


    tf::Vector3 target_pose_t(target_xyz_pose.x, target_xyz_pose.y, target_xyz_pose.z);
    tf::Quaternion target_pose_q;
    target_pose_q.setRPY(target_xyz_pose.rx, target_xyz_pose.ry, target_xyz_pose.rz);
    tf::Transform target_pose_tf(target_pose_q, target_pose_t);

    tf::Transform relative_transform = reference_pose_tf.inverse() * target_pose_tf;


    relative_transform.getOrigin().x();
    relative_transform.getOrigin().y();
    relative_transform.getOrigin().z();

    double roll, pitch, yaw;
    relative_transform.getBasis().getRPY(roll, pitch, yaw);

    float delta_x = relative_transform.getOrigin().x();
    float delta_y = relative_transform.getOrigin().y();
    float delta_z = relative_transform.getOrigin().z();

    float roll_deg = roll * 180.0 / M_PI;
    float pitch_deg = pitch * 180.0 / M_PI;
    float yaw_deg = yaw * 180.0 / M_PI;

//    qDebug() << "relative_transform delta x " << delta_x;
//    qDebug() << "relative_transform delta y " << delta_y;
//    qDebug() << "relative_transform delta z " << delta_z;

//    qDebug() << "relative_transform delta roll " <<  roll_deg;
//    qDebug() << "relative_transform delta pitch " <<  pitch_deg;
//    qDebug() << "relative_transform delta yaw " <<  yaw_deg;

    if(std::abs(delta_x) < 20
            && std::abs(delta_y) < 20
            && std::abs(delta_z) < 20
            && std::abs(roll_deg) < 2
            && std::abs(pitch_deg) < 2
            && std::abs(yaw_deg) < 2)
    {
        return true;
    }
    else
    {
        return false;
    }

}

    bool RobotArm::is_remote_mode(){

        if(robot_arm_monitor_->robot_mode == 2){
            return true;
        }
        else{
            return false;
        }
    }

    //增加路点信息
    bool RobotArm::add_common_waypoint(QString pose_name, MoveType type, int speed, int circular_radius){


        if(robot_common_poses_.find(pose_name) == robot_common_poses_.end()){
            return false;
        }

        RobotPoseJoint pose = robot_common_poses_.at(pose_name);

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
        QJsonArray targetPositionByJoint = {pose.joint1,
                                            pose.joint2,
                                            pose.joint3,
                                            pose.joint4,
                                            pose.joint5,
                                            pose.joint6};

        int move_type = 0;
        if(type == MoveType::JOINT){
            move_type = 0;
        }
        else if(type == MoveType::LINE){
            move_type = 1;
        }

        QJsonObject params = {{"circular_radius", circular_radius},
                              {"speed", speed},
                              //{"dec", 10},
                              {"moveType", move_type},
                              {"wayPoint",targetPositionByJoint}};

        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("addPathPoint"));

        return execSpecifiedCommand(JsonCommand);
    }


    //增加机台路点信息
    bool RobotArm::add_station_waypoint(QString station_id,
                             QString pose_name,
                             MoveType type,
                             int speed,
                             int circular_radius){


        if(robot_station_poses_.find(station_id) == robot_station_poses_.end()){
            return false;
        }

        auto station_poses = robot_station_poses_.at(station_id);

        if(station_poses.find(pose_name) == station_poses.end()){
            return false;
        }

        RobotPoseXYZ pose_xyz = station_poses.at(pose_name);

        //空间直角坐标转换到关节坐标
        RobotPoseJoint joint_pose;
        if(!getIK(pose_xyz,joint_pose)){
            return false;
        }

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
        QJsonArray targetPositionByJoint = {joint_pose.joint1,
                                            joint_pose.joint2,
                                            joint_pose.joint3,
                                            joint_pose.joint4,
                                            joint_pose.joint5,
                                            joint_pose.joint6};

        int move_type = 0;
        if(type == MoveType::JOINT){
            move_type = 0;
        }
        else if(type == MoveType::LINE){
            move_type = 1;
        }

        QJsonObject params = {{"circular_radius", circular_radius},
                              {"speed", speed},
                              //{"dec", 10},
                              {"moveType", move_type},
                              {"wayPoint",targetPositionByJoint}};

        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("addPathPoint"));

        return execSpecifiedCommand(JsonCommand);


    }

    bool RobotArm::add_xyz_waypoint(RobotPoseXYZ pose,
                             MoveType type,
                             int speed,
                             int circular_radius){

        //空间直角坐标转换到关节坐标
        RobotPoseJoint joint_pose;
        if(!getIK(pose,joint_pose)){
            return false;
        }

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
        QJsonArray targetPositionByJoint = {joint_pose.joint1,
                                            joint_pose.joint2,
                                            joint_pose.joint3,
                                            joint_pose.joint4,
                                            joint_pose.joint5,
                                            joint_pose.joint6};

        int move_type = 0;
        if(type == MoveType::JOINT){
            move_type = 0;
        }
        else if(type == MoveType::LINE){
            move_type = 1;
        }

        QJsonObject params = {{"circular_radius", circular_radius},
                              {"speed", speed},
                              //{"dec", 10},
                              {"moveType", move_type},
                              {"wayPoint",targetPositionByJoint}};

        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("addPathPoint"));

        return execSpecifiedCommand(JsonCommand);

    }

    bool RobotArm::add_joint_waypoint(RobotPoseJoint pose,
                             MoveType type,
                             int speed,
                             int circular_radius){

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};
        QJsonArray targetPositionByJoint = {pose.joint1,
                                            pose.joint2,
                                            pose.joint3,
                                            pose.joint4,
                                            pose.joint5,
                                            pose.joint6};

        int move_type = 0;
        if(type == MoveType::JOINT){
            move_type = 0;
        }
        else if(type == MoveType::LINE){
            move_type = 1;
        }

        QJsonObject params = {{"circular_radius", circular_radius},
                              {"speed", speed},
                              //{"dec", 10},
                              {"moveType", move_type},
                              {"wayPoint",targetPositionByJoint}};

        JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("addPathPoint"));

        return execSpecifiedCommand(JsonCommand);

    }



    //清除路点信息
    bool RobotArm::clear_waypoints(){

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

        //JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("clearPathPoint"));

        return execSpecifiedCommand(JsonCommand);

    }
    //执行轨迹运动
    bool RobotArm::move_by_path(BlockMotion blockmotion){

        QJsonObject JsonCommand = {{"jsonrpc",QJsonValue("2.0")},{"id",QJsonValue(1)}};

        //JsonCommand.insert("params",QJsonValue(params));
        JsonCommand.insert("method",QJsonValue("moveByPath"));

        bool state = execMoveByPathCommand(JsonCommand);

        if(state == false){
            return false;
        }


        //阻塞方式
        robot_sleep(200);
        if(blockmotion == BlockMotion::LOCKED){

            while(1){
                robot_sleep(100);
                if(robot_arm_monitor_->robot_state == 0){
                    break;
                }

                //急停检测
                if(robot_arm_monitor_->robot_state == 2){
                    //qDebug() << "AGV急停按钮按下，停止自动运行";
                    return false;
                }

                //远程手自动切换
                if(robot_arm_monitor_->robot_mode != 2){
                    QMessageBox::warning(nullptr, "提示\t", "切换至手动模式，停止自动运行\t");
                    return false;
                }

                //AGV急停按下
                if(agv_emergency_stop == true){
                     qDebug() << "AGV急停按钮按下，停止自动运行";
                    //QMessageBox::warning(nullptr, "提示\t", "AGV急停按下，停止自动运行\t");
                    return false;
                }
            }
        }

        //清除路点
        return clear_waypoints();


    }


   void RobotArm::set_agv_emergency_state(bool state){
       agv_emergency_stop = state;
   }



